;  W9GR autonotcher
;
;  Ported for the TI 320C26 DSK by Johan Forrer KC7WW Jan. 1994
;  Bug fixes reported by HB8JNX re: SUBK 1,11
;  Added LED outputs
;
;  Added better use of Assembler directives, 
;  Gerrit Polder, PA3BYA, Apr. 1994
;  You now can switch between lms or denoiser by setting BIO_Z.
;  If you don't have a led display connected you can disable
;  the bargraph software by setting LEDDISP 0.
;  This results in compacter code, and you can run the filter
;  in a conf 1 memory configuration.
;
;  Added left and right bargraph, left output, right input signal
;  gp 16/6/95
;
;* Filter length = 24 Delay length = 65
;* Both notcher and denoiser functions, selected by BIO switch
;* LMS coefficients are decayed one at a time per program loop

;--------------------------------------------------------------------------
;* BIO=1 > AUTOMATIC NOTCH
;* BIO=0 > DENOISING FUNCTION
BIO_Z	.set	1

LEDDISP	.set	1	; LED DISPLAY attached or not ====GP====

NFIR	.set    24      ; LENGTH OF WIENER FIR FILTER
NFIR_1	.set	23
NDEL	.set    65      ; LONG DELAY FOR NOTCHER
NDEL_1	.set	64
SDEL	.set	1       ; SHORT DELAY FOR DENOISER MODE
SDEL_1	.set 	0

CONF_1	.set	0
CONF_2  .set	1
 .if CONF_1
	.ds	0x400	; beginning of data
 .endif
 .if CONF_2 
	.ds	0x600	; beginning of data
 .endif
;    			; AIC working parameters
;-----------------------------------------------------------------------------
TA	.set    20      ;
RA	.set    20      ; 
TAp	.set    1       ;
RAp	.set    1       ;
TB	.set    15      ;
RB	.set    15      ;
AIC_CMD	.set    028h	; bit    0     =enable/disable input bandpass
			;        1     =enable/disable loopback
			;        2     =enable/disable AUX input
			;        3     =sync I/O
			; bits 5&4: 11 =+/-6V    
			;           01h=+/-3V
			;           10h=+/-1.5V
;-----PAGE 12----------------------------------------------------------------
NBETA:   .word     01000h	; 01000 BETA VALUE (NOTCH)
NDECAY:  .word     07FE4h  	; 07FE4 DECAY VALUE (NOTCH) (>8000 => NO DECAY)
CBETA:   .word     01800h  	; BETA VALUE (DENOISE)
CDECAY:  .word     07D80h  	; DECAY VALUE (DENOISE) (>8000 => NO DECAY)
ONES:    .word     0FFFFh  	; ALL ONES FOR EX-OR
;
; INPUT SIGNAL DELAY (NOTCH) - 65 taps
;

X:      .word   0      	; FILTER DELAY (NOTCH)
X1:	.word	0
X2:	.word	0
X3:	.word	0
X4:	.word	0
X5:	.word	0
X6:	.word	0
X7:	.word	0
X8:	.word	0
X9:	.word	0
X10:	.word	0
X11:	.word	0
X12:	.word	0
X13:	.word	0
X14:	.word	0
X15:	.word	0
X16:	.word	0
X17:	.word	0
X18:	.word	0
X19:	.word	0
X20:	.word	0
X21:	.word	0
X22:	.word	0
X23:	.word	0

W:      .word   0      	; LMS COEFFICIENTS
W1:	.word	0
W2:	.word	0
W3:	.word	0
W4:	.word	0
W5:	.word	0
W6:	.word	0
W7:	.word	0
W8:	.word	0
W9:	.word	0
W10:	.word	0
W11:	.word	0
W12:	.word	0
W13:	.word	0
W14:	.word	0
W15:	.word	0
W16:	.word	0
W17:	.word	0
W18:	.word	0
W19:	.word	0
W20:	.word	0
W21:	.word	0
W22:	.word	0
W23:	.word	0

D:       .word     0, 0, 0, 0, 0, 0, 0, 0, 0, 0 
D10:     .word     0, 0, 0, 0, 0, 0, 0, 0, 0, 0
D20:     .word     0, 0, 0, 0, 0, 0, 0, 0, 0, 0
D30:     .word     0, 0, 0, 0, 0, 0, 0, 0, 0, 0
D40:     .word     0, 0, 0, 0, 0, 0, 0, 0, 0, 0
D50:     .word     0, 0, 0, 0, 0, 0, 0, 0, 0, 0
D60:     .word     0, 0, 0, 0
D64:     .word     0

BETA:    .word     0       	; BETA VALUE (NOTCH)
DECAY:   .word     0       	; DECAY VALUE (>8000 => NO DECAY)
ONE:     .word     1       	; CONSTANT 1
Y:       .word     0       	; WIENER FIR FILTER OUTPUT
E:       .word     0       	; ERROR SIGNAL
EBETA:   .word     0       	; E*BETA
SQUARE:  .word     0       	; INPUT SQUARED
LED:     .word     0       	; LED DISPLAY
TEMP:    .word     0       	; TEMPORARY STORAGE
HPFOUT:  .word     0       	; HPF OUTPUT
TEMP1:    .word     0       	; TEMPORARY STORAGE
SQUARE1:  .word     0       	; INPUT SQUARED
LED1:     .word     0       	; LED DISPLAY
ONE1:     .word     1       	; CONSTANT 1
;-----PAGE 13----------------------------------------------------------------
;
;* 0.5 DB/40 DB ELLIPTIC IIR HPF
;* PASS 300 HZ REJ 35 HZ
;* HPF COEFFICIENTS:
HPF:    .word  3644	;HPF NETWORK COEFFICIENTS
HPF1:	.word -7288
HPF2:	.word  3644
HPF3:	.word  7700
HPF4:	.word -3644

INP:    .word   0       ;INPUT DELAY - USED FOR HPF
INP1:	.word	0
INP2:	.word	0

DENOM:   .word  0    	;INPUT HPF DENOMINATOR DELAY
DENOM1:  .word	0
;----------------------------------------------------------------------------

	.include  "mmregs.asm"
        .ps     0xFA0A          ;
        B	RINT            ;FA0A  RINT
        .ps     0xFA0C          ;
	B	XINT		;FA0C  XINT

***********************************************************************
        .ps     0xFB00		; Bootloader transfers here
	.entry                  ; This is a must!


	zac	
	ldpk	IMR		; IMR disable any further interrupts
	sacl	IMR

	ssxm			; set sign extention mode 
	SOVM
* Reset and initialze the AIC
	call	AIC_SET 

	LARP    AR0
	lark    AR0,NDEL_1
	lark    AR1,NFIR_1

	eint			; Turn interrupts on

;* MAIN PROGRAM LOOP

AGAIN:  idle
	B	AGAIN

RINT
	dint

        ldpk     DRR		; point to memory mapped serial port recieve reg
        lac      DRR         ; load accumulator with word received from AIC
;        sfr                  ; shift right twice to remove unused lsb's
;        sfr                  ;

	ldpk	INP		; this sets page 13
	SACL    INP          ; save input sample

;* INPUT HIGHPASS FILTER:

;*       NUMERATOR	     ; all page 13 stuff

	ZAC
	LT      INP2
	MPY     HPF2
	LTD     INP1
	MPY     HPF1
	LTD     INP
	MPY     HPF

;*       DENOMINATOR

	LTA     DENOM1
	MPY     HPF4
	LTD     DENOM
	MPY     HPF3
	APAC

	ADLK    1,11            ;ADD 0.5 TO ROUND    ====JF====
	SACH    DENOM,4         ;SAVE WITH SHIFT

	LDPK    HPFOUT		; HPFOUT
	SACH    HPFOUT,4        ;STORE ON PAGE 12 TOO

	SAR     AR0,TEMP        ;STORE AR0
	lalk    D               ;LOAD ACCUM WITH OFFSET
	ADD     TEMP            ;ADD AR0 VALUE
	SACL    LED             ;STORE USING LED AS TEMP LOCATION
	LAR     AR0,LED         ;PUT INTO AR0
	ZALH    HPFOUT          ;LOAD ACCUM WITH HPF OUTPUT
	SACH    *,0             ;SAVE HPF OUTPUT IN DELAY
	LAR     AR0,TEMP        ;RESTORE AR0

;* DECAY JUST ONE LMS COEFFICIENT (NOTCH)
;* USE AR1 TO POINT TO COEFFICIENT TO BE DECAYED
;* TEMPORARILY ADD W TO AR0 CONTENTS THEN RESTORE

	LARP    AR1             ;USE AR1
	SAR     AR1,TEMP        ;STORE AR1
	lalk    W               ;LOAD ACCUM WITH OFFSET
	ADD     TEMP            ;ADD AR1 VALUE
	SACL    LED             ;STORE USING LED AS TEMP LOCATION
	LAR     AR1,LED         ;PUT INTO AR1
	LT      DECAY
	MPY     *               ;MULTIPLY
	PAC
	SACH    *,1             ;PUT BACK
	LAR     AR1,TEMP        ;RESTORE AR1
	BANZ    ARDECAY,*-
	LARK    AR1,NFIR_1
ARDECAY:
  .if LEDDISP			; ====GP====
	ldpk	HPFOUT
	ZALH    HPFOUT		; display on LED Display
	CALL	BARGRA1
  .endif
	ldpk	HPFOUT

;* DATA DELAY MOVE

	LARP    AR0
	BANZ    TWEAK,*-

  .if BIO_Z			; ====GP====
	LARK    AR0,NDEL_1      ;LOAD AR0 FOR LONG (AUTONOTCH) DELAY
  .else
	LARK    AR0,SDEL_1      ;LOAD AR0 FOR SHORT (DENOISER) DELAY
  .endif

;* TWEAK NOTCH COEFFICIENTS, BEFORE DOING FILTERING/DATA MOVE:

TWEAK:  
        larp     AR3
        lrlk     AR3,X23  	; load AR3 with address of last delay element
	lrlk     AR4,W23      	; load AR4 with address of last filter coef
        lark     AR5,23       	; load AR5 with loop counter

	LT      EBETA

shf:   	MPY	 *-,AR4		; ARP=3 (Xn)
	PAC
        ADDH     *,AR4		; ARP=4
        SACH     *-,0,AR5	; ARP=4
        banz     shf,*-,AR3	; ARP=5

;* NOTCHER/DENOISER FIR WIENER FILTER:

        larp     AR3
        lrlk     AR3,X23  	; load AR3 with address of last delay element
	lrlk     AR4,W23      	; load AR4 with address of last filter coef
        lark     AR5,22       	; load AR5 with loop counter
        zac
        lt       *-,AR4
        mpy      *-,AR3

lpf:   	ltd	 *-,AR4		; ARP=3 (Xn)
        mpy      *-,AR5		; ARP=4
        banz     lpf,*-,AR3	; ARP=5
        apac

	ADLK    1,15          ;ADD 0.5 TO ROUND	    ===JF===
	SACH    Y,0

;* E = D - Y

	ZALH    HPFOUT
	SUBH    Y
	SACH    E

	LT      E
	MPY     BETA
	PAC
	SACH    EBETA

;* TAKE DATA FROM DELAY LINE AND PUT IT IN FIR FILTER

	larp	AR0				; use AR0
	SAR     AR0,TEMP        ;STORE AR0
	lalk    D               ;LOAD ACCUM WITH OFFSET
	ADD     TEMP            ;ADD AR0 VALUE
	SACL    LED             ;STORE USING LED AS TEMP LOCATION
	LAR     AR0,LED         ;PUT INTO AR0
	ZALH    *
	SACH    X
	LAR     AR0,TEMP        ;RESTORE AR0


  .if BIO_Z			; ====GP====

;* OUTPUT NOTCHED SIGNAL TO DAC

DENOTCH:
	ZALS    E               ;NOTCHED OUTPUT SIGNAL
	call	DA_DISP
  .if LEDDISP			; ====GP====
  	ldpk	E
	ZALH	E
	CALL	BARGRA
  .endif
	
;* LOAD NOTCH VALUES FOR DECAY AND BETA
	ldpk	NDECAY
	ZALH    NDECAY
	SACH    DECAY
	ZALH    NBETA
	SACH    BETA

	eint
	ret

  .else				; ====GP====

;* OUTPUT DENOISED SIGNAL TO DAC

DENOISE: ZALS    Y               ;CORRELATOR (DENOISED)
	call	DA_DISP

;* LOAD DENOISE VALUES FOR DECAY AND BETA
	
	ldpk	CDECAY
	ZALH    CDECAY
	SACH    DECAY
	ZALH    CBETA
	SACH    BETA

	eint
	ret
  .endif			; ====GP====

;--------------------------------------------------------------------------
XINT	ret
****************************************************************************
* Routine to set up AIC
****************************************************************************
AIC_SET: 
*
* Reset AIC by defining some GLOBAL memory space, then
* reading it. 
* 
	dint                    ; Normally after reset interrupts are disabled
	ldpk	DXR		; DXR
	lack    0x80            ; AIC reset by pulsing /BR (Global Data)
	sach    DXR             ; send 0 to DXR (AIC)
	sacl    GREG            ; 256 * 100 nS /BR pulse
	lrlk    AR0,0xFFFF      ;
	rptk    255             ; read junk from address 0xFFFF
	lac     *,0,AR0         ;

 .if CONF_1
	conf    1               ; B0    is code (FA00 - FBFF)
				; B1,B3 is data (0400 - 07FF)
 .endif
 .if CONF_2
	conf    2               ; B0,B1 is code (FA00 - FD00)
 .endif

;----------------------------------------------------------------------------
* Configure AIC.
* The transmit interrupt must be used to sense when
* the AIC has processed the commands that we are sending
* to it. Since interrupts have been disabled at entry, arm
* the transmit interrupt first.
*
	LDPK	IMR
	LAC	IMR
	ORK	020h
	SACL	IMR
        eint
;-----------------------command 00 = TX/RX A-cntrs <= TA/TB
        lalk   	TA,9
        adlk   	RA,2
        call   	AIC_2nd
;-----------------------command 01 = TX/RX A-cntrs <= TA+TA'/TB+TB'
        lalk   	TAp,9
        adlk   	RAp,2
        addk   	01h
        call   	AIC_2nd
;-----------------------command 10 = TX/RX A-cntrs <= TA-TA'/TB-TB'
        lalk   	TB,9
        adlk   	RB,2
        addk   	02h
        call   	AIC_2nd
;-----------------------command 11 = set command register
        lalk   	AIC_CMD,2
        addk   	03h
        call   	AIC_2nd
;------------------------

        nop 			; Introduce a brief delay (5*tics)
        nop
        nop
        nop
        nop

        ssxm			; Allows sign extention during shifts
        spm     0		; Disable shifts on P-reg 

* AIC is now initialized. Disable XINT, then leave only the RINT activated.

	dint
	LDPK	IMR
	LAC	IMR
	andk	0xFFCF
	ORK	010h
	SACL	IMR
	ret
;----------------------------------------------------------------
AIC_2nd: ldpk   DXR               ;
         sach   DXR               ;
         idle                     ;
         adlk   6,15              ;0000 0000 0000 0011 XXXX XXXX XXXX XXXX b
         sach   DXR               ;
         idle                     ;ACCU_hi requests 2nd XMIT
         sacl   DXR               ;
         idle                     ;ACCU_lo sets up registers
         zac                      ;
         sacl   DXR               ;make sure the word got sent
         idle                     ;
         ret                      ;

*---------------------------------------------------
* Write accumulator to D/A
*---------------------------------------------------
DA_DISP:
;	sfl
;	sfl
	andk     0fffch
	ldpk     DXR
	sacl     DXR         ; write output word to transmit register
	ret
;----------------------------------------------------------------------------
* SQUARE INPUT FOR LED DISPLAY, 3DB/SEGMENT

  .if LEDDISP			; ====GP====

BARGRA: 
	ldpk	TEMP 
	SACH    TEMP
	LT      TEMP
	MPY     TEMP
	PAC
	SACH    TEMP
	SUBH    SQUARE
	BGZ     PEAK
	ZALH    SQUARE
	SUBH    ONE
	ABS
	SACH    SQUARE
	B       DISPLAY

PEAK    ZALH    TEMP
	SACH    SQUARE

* CONVERT TO DISPLAY BY BITWISE OR

DISPLAY LAC     SQUARE,10
	SACH    LED

	LAC     SQUARE,9
	SACH    TEMP
	ZALS    TEMP
	OR      LED
	SACL    LED

	LAC     SQUARE,8
	SACH    TEMP
	ZALS    TEMP
	OR      LED
	SACL    LED

	LAC     SQUARE,7
	SACH    TEMP
	ZALS    TEMP
	OR      LED
	SACL    LED

	LAC     SQUARE,6
	SACH    TEMP
	ZALS    TEMP
	OR      LED
	SACL    LED

	LAC     SQUARE,5
	SACH    TEMP
	ZALS    TEMP
	OR      LED
	SACL    LED

	LAC     SQUARE,4
	SACH    TEMP
	ZALS    TEMP
	OR      LED
	SACL    LED

	LAC     SQUARE,3
	SACH    TEMP
	ZALS    TEMP
	OR      LED
	
	cmpl    			; invert bits
	SACL    LED
	
	OUT     LED,0			; PA0
	ret
	
; display right leds
BARGRA1: 
	ldpk	TEMP1 
	SACH    TEMP1
	LT      TEMP1
	MPY     TEMP1
	PAC
	SACH    TEMP1
	SUBH    SQUARE1
	BGZ     PEAK1
	ZALH    SQUARE1
	SUBH    ONE1
	ABS
	SACH    SQUARE1
	B       DISPLAY1

PEAK1    ZALH    TEMP1
	SACH    SQUARE1

* CONVERT TO DISPLAY BY BITWISE OR

DISPLAY1 LAC     SQUARE1,10
	SACH    LED1

	LAC     SQUARE1,9
	SACH    TEMP1
	ZALS    TEMP1
	OR      LED1
	SACL    LED1

	LAC     SQUARE1,8
	SACH    TEMP1
	ZALS    TEMP1
	OR      LED1
	SACL    LED1

	LAC     SQUARE1,7
	SACH    TEMP1
	ZALS    TEMP1
	OR      LED1
	SACL    LED1

	LAC     SQUARE1,6
	SACH    TEMP1
	ZALS    TEMP1
	OR      LED1
	SACL    LED1

	LAC     SQUARE1,5
	SACH    TEMP1
	ZALS    TEMP1
	OR      LED1
	SACL    LED1

	LAC     SQUARE1,4
	SACH    TEMP1
	ZALS    TEMP1
	OR      LED1
	SACL    LED1

	LAC     SQUARE1,3
	SACH    TEMP1
	ZALS    TEMP1
	OR      LED1
	sfl
	sfl
	sfl
	sfl
	sfl
	sfl
	sfl
	sfl

	cmpl    			; invert bits
	SACL    LED1
	
	OUT     LED1,0			; PA0
	ret

  .endif
;-------------------------------------------------------------------------
	.end
